
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/sleep.h>
#include <inttypes.h>
#include <util/delay.h>

#include "uart.h"
#include "rprintf.h"
#include "buffer.h"
#include "axis.h"
#include "stepmotor.h"


#define END_OF_COMMAND  ';'


// util functions 
static int pop_int_from_buffer(cBuffer* buf){
	int sign = 1;
	int val = 0;
	unsigned char ch;

	while(buf->datalength > 0){
		ch = bufferGetFromFront(buf);
		if(ch == '-'){
			sign = -1;
			break;
		}
		else if(ch == '+'){
			sign = +1;
			break;
		}
		else if('0' <= ch && ch <= '9'){
			val = ch - '0';
			break;
		}
	}
	while(buf->datalength > 0){
		ch = buf->dataptr[buf->dataindex];
		if('0' <= ch && ch <= '9'){
			val = (val * 10) + (ch - '0');
		}
		else{
			break;
		}
		(void)bufferGetFromFront(buf);
	}

	return sign * val;
}


static void flush_one_line_from_buffer(cBuffer* buf){
	unsigned char ch;
	while(buf->datalength > 0){
		ch = bufferGetFromFront(buf);
		if(ch == END_OF_COMMAND){
			break;
		}
	}
}


// uart callback func
static unsigned char uart_num_command = 0;
static void uartRxHandler(unsigned char c)
{
	if(c == END_OF_COMMAND){
		uart_num_command++;
	}
}


int main(void)
{
	axis axis_x, axis_y, axis_z;

	uartInit();
	uartSetBaudRate(9600);
	rprintfInit(uartSendByte);
	uartSetRxHandler(uartRxHandler);

	axis_init(&axis_x);
	axis_init(&axis_y);
	axis_init(&axis_z);
	axis_enable(&axis_x);
	axis_enable(&axis_y);
	axis_enable(&axis_z);

	outb(DDRB, 0x3F);
	outb(DDRC, 0x3F);

	rprintf("servosoft\n");

	// main loop
	while(TRUE){
		if(uart_num_command > 0){
			cBuffer* read_buf = uartGetRxBuffer();
			unsigned char command = bufferGetFromFront(read_buf);

			//rprintf("cmd=%c\n", command);

			switch(command){
			case 's':   // set
				switch(bufferGetFromFront(read_buf)){
				case 'g': //goal position
					axis_set_goal_pos(&axis_x, pop_int_from_buffer(read_buf));
					axis_set_goal_pos(&axis_y, pop_int_from_buffer(read_buf));
					axis_set_goal_pos(&axis_z, pop_int_from_buffer(read_buf));
					break;
				case 'v':  // inv velocity
					axis_set_inv_velocity(&axis_x, pop_int_from_buffer(read_buf));
					axis_set_inv_velocity(&axis_y, pop_int_from_buffer(read_buf));
					axis_set_inv_velocity(&axis_z, pop_int_from_buffer(read_buf));
					break;
				}
				break;
			case 'g':   // get current Status
				switch(bufferGetFromFront(read_buf)){
				case 'c':
					rprintf("%d %d %d\n", axis_x.current_pos, axis_y.current_pos, axis_z.current_pos);
					break;
				case 'g':
					rprintf("%d %d %d\n", axis_x.goal_pos, axis_y.goal_pos, axis_z.goal_pos);
					break;
				case 'v':
					rprintf("%d %d %d\n", axis_x.inv_velocity, axis_y.inv_velocity, axis_z.inv_velocity);
					break;
				case 'i':
					rprintf("%d %d %d\n", axis_x.in_position, axis_y.in_position, axis_z.in_position);
					break;
				}
				break;
			}

			flush_one_line_from_buffer(read_buf);
			uart_num_command--;
		}

		axis_do_one_control_cycle(&axis_x);
		axis_do_one_control_cycle(&axis_y);
		axis_do_one_control_cycle(&axis_z);

		{
			char b, c;
			b  = 0x0F & (axis_x.motor.output);
			b |= 0x30 & (axis_y.motor.output << 4);
			c  = 0x03 & (axis_y.motor.output >> 2);
			c |= 0x3C & (axis_z.motor.output << 2);
			outb(PORTB, b);
			outb(PORTB, b);
			outb(PORTC, c);
			outb(PORTC, c);
		}
	}

	return 0;

}
